/*************************************************
*      Filename: moving_point.cu
*        Coding: utf-8
*        Author: Futuan Li
*         Email: lifutuan@galasports.net
*      HomePage: http://www.galasports.net
*  Copyright(C): InnoReal Technology Co.,Ltd.
*       Version: 0.0.1
*    CreateDate: 2018-04-25 11:54:35
*    LastChange: 2018-06-26 17:34:02
*          Desc: 
*       History: 
*************************************************/
#include "moving_point.h"
#include <algorithm>
#include <innoreal/utils/innoreal_helper.hpp>
#include <innoreal/utils/innoreal_timer.hpp>

// Moving_Point_Auto函数的作用是给出最大距离位置的编号,这里的函数是下述所有算法的初始化,input: data(关键帧的数据), dis_eva(sf_given X 2的距离数据);
// output: keyframe(关键帧的数据，目的是存储在类中，方便在没有删除指针或对象时可以重复使用有些数据),
// distance_eva_col0(顺序的距离数据),distance_eva_col1(逆序的距离数据).
void Moving_Point::Moving_Point_Auto(std::vector<int> data, std::vector<std::vector<double> > dis_eva)
{
	keyframe.resize(data.size());
	distance_eva_col0.resize(data.size());	
	distance_eva_col1.resize(data.size());	
	for(int i = 0; i < data.size(); i++) {
		distance_eva_col0[i] = dis_eva[i][0];
		distance_eva_col1[i] = dis_eva[i][1];
		keyframe[i] = data[i];
	}
	//output_vector.Vec1dint_Output(keyframe);
	//output_vector.Vec1ddouble_Output(distance_eva_col0);
	//output_vector.Vec1ddouble_Output(distance_eva_col1);
  /*
	auto maxPosition0 = max_element(distance_eva_col0.begin(), distance_eva_col0.end());
	auto maxPosition1 = max_element(distance_eva_col1.begin(), distance_eva_col1.end());	
	if(distance_eva_col0[maxPosition0 - distance_eva_col0.begin()] > distance_eva_col1[maxPosition1 - distance_eva_col1.begin()]){
			keyframe[maxPosition0 - distance_eva_col0.begin()] = data[maxPosition0 - distance_eva_col0.begin()] - 1;
			dis_max_value_font = distance_eva_col0[maxPosition0 - distance_eva_col0.begin()];
	}
	else {
			keyframe[maxPosition1 - distance_eva_col1.begin()] = data[maxPosition1 - distance_eva_col1.begin()] + 1;
			dis_max_value_font = distance_eva_col1[maxPosition1 - distance_eva_col1.begin()];
	}
	*/
//	output_vector.Vec1dint_Output(keyframe);//标号
//	output_vector.Vec1ddouble_Output(distance_eva_col0);//第一列
//	output_vector.Vec1ddouble_Output(distance_eva_col1);//第二列
}

// Moving_Point_Update函数作用是获得关键帧的具体编号.
void Moving_Point::Moving_Point_Update(int sf_sum)
{
    Calculate_Total_Evalation *calculate_total_evalation = new Calculate_Total_Evalation;
    auto maxPosition0 = max_element(distance_eva_col0.begin(), distance_eva_col0.end());
    auto maxPosition1 = max_element(distance_eva_col1.begin(), distance_eva_col1.end());

    if(distance_eva_col0[maxPosition0 - distance_eva_col0.begin()] > distance_eva_col1[maxPosition1 - distance_eva_col1.begin()]){
        //选择第一列的最大的距离
        keyframe[maxPosition0 - distance_eva_col0.begin()] = keyframe[maxPosition0 - distance_eva_col0.begin()] - 1;

        if(maxPosition0 - distance_eva_col0.begin() == 0) {//最大距离所在的位置是第一关键帧的情况，更新三个值
            if(keyframe[maxPosition0 - distance_eva_col0.begin()] == 1) { //移动后的关键帧的位置是1,此时与0是相邻的
                distance_eva_col0[maxPosition0 - distance_eva_col0.begin()] = 0;

                calculate_total_evalation->Cal_Evalation_Update(keyframe[maxPosition0 - distance_eva_col0.begin()],
                                                                keyframe[maxPosition0 - distance_eva_col0.begin() + 1], sf_sum);
                distance_eva_col0[maxPosition0 - distance_eva_col0.begin() + 1] = calculate_total_evalation->evalate_dis_one[0];
                distance_eva_col1[maxPosition0 - distance_eva_col0.begin()] = calculate_total_evalation->evalate_dis_one[1];
            }
            else {//最大距离所在的位置不是第一帧的情况
                calculate_total_evalation->Cal_Evalation_Update(0, keyframe[maxPosition0 - distance_eva_col0.begin() + 1], sf_sum);
                distance_eva_col0[maxPosition0 - distance_eva_col0.begin()] = calculate_total_evalation->evalate_dis_one[0];

                calculate_total_evalation->Cal_Evalation_Update(keyframe[maxPosition0 - distance_eva_col0.begin()],
                                                                keyframe[maxPosition0 - distance_eva_col0.begin() + 1], sf_sum);
                distance_eva_col0[maxPosition0 - distance_eva_col0.begin() + 1] = calculate_total_evalation->evalate_dis_one[0];
                distance_eva_col1[maxPosition0 - distance_eva_col0.begin()] = calculate_total_evalation->evalate_dis_one[1];
            }
        }
        else if(maxPosition0 - distance_eva_col0.begin() == keyframe.size() - 1) {//最大距离所在的位置是最后一关键帧的情况，更新四个值
            if(keyframe[maxPosition0 - distance_eva_col0.begin()] - keyframe[maxPosition0 - distance_eva_col0.begin() - 1] == 1) {
                //移动后的关键帧的位置和上一关键帧的位置为相邻的位置
                distance_eva_col0[maxPosition0 - distance_eva_col0.begin()] = 0;
                distance_eva_col1[maxPosition0 - distance_eva_col0.begin() - 1] = 0;

                calculate_total_evalation->Cal_Evalation_Update(keyframe[maxPosition0 - distance_eva_col0.begin()] - 1, sf_sum, sf_sum);
                distance_eva_col1[maxPosition0 - distance_eva_col0.begin()] = calculate_total_evalation->evalate_dis_one[1];
            }
            else { //移动的关键帧的位置和上一帧的位置不是相邻的
                calculate_total_evalation->Cal_Evalation_Update(keyframe[maxPosition0 - distance_eva_col0.begin() -1],
                                                                keyframe[maxPosition0 - distance_eva_col0.begin()], sf_sum);
                distance_eva_col0[maxPosition0 - distance_eva_col0.begin()] = calculate_total_evalation->evalate_dis_one[0];
                distance_eva_col1[maxPosition0 - distance_eva_col0.begin() - 1] = calculate_total_evalation->evalate_dis_one[1];

                calculate_total_evalation->Cal_Evalation_Update(keyframe[maxPosition0 - distance_eva_col0.begin()]-1, sf_sum, sf_sum);
                distance_eva_col1[maxPosition0 - distance_eva_col0.begin()] = calculate_total_evalation->evalate_dis_one[1];
            }
        }
        else {//最大距离既不是第一关键帧也不是最后一关键帧的情况下
            if(keyframe[maxPosition0 - distance_eva_col0.begin()] - keyframe[maxPosition0 - distance_eva_col0.begin() - 1] == 1) {
                distance_eva_col0[maxPosition0 - distance_eva_col0.begin()] = 0;
                distance_eva_col1[maxPosition0 - distance_eva_col0.begin() - 1] = 0;

                calculate_total_evalation->Cal_Evalation_Update(keyframe[maxPosition0 - distance_eva_col0.begin()],
                                                                keyframe[maxPosition0 - distance_eva_col0.begin() + 1], sf_sum);
                distance_eva_col0[maxPosition0 - distance_eva_col0.begin() + 1] = calculate_total_evalation->evalate_dis_one[0];
                distance_eva_col1[maxPosition0 - distance_eva_col0.begin()] = calculate_total_evalation->evalate_dis_one[1];
            }
            else {
                calculate_total_evalation->Cal_Evalation_Update(keyframe[maxPosition0 - distance_eva_col0.begin() -1],
                                                                keyframe[maxPosition0 - distance_eva_col0.begin()], sf_sum);
                distance_eva_col0[maxPosition0 - distance_eva_col0.begin()] = calculate_total_evalation->evalate_dis_one[0];
                distance_eva_col1[maxPosition0 - distance_eva_col0.begin() - 1] = calculate_total_evalation->evalate_dis_one[1];

                calculate_total_evalation->Cal_Evalation_Update(keyframe[maxPosition0 - distance_eva_col0.begin()],
                                                                keyframe[maxPosition0 - distance_eva_col0.begin() + 1], sf_sum);
                distance_eva_col0[maxPosition0 - distance_eva_col0.begin() + 1] = calculate_total_evalation->evalate_dis_one[0];
                distance_eva_col1[maxPosition0 - distance_eva_col0.begin()] = calculate_total_evalation->evalate_dis_one[1];
            }
        }
        auto maxPos0 = max_element(distance_eva_col0.begin(), distance_eva_col0.end());
        auto maxPos1 = max_element(distance_eva_col1.begin(), distance_eva_col1.end());
        if(distance_eva_col0[maxPos0 - distance_eva_col0.begin()] > distance_eva_col1[maxPos1 - distance_eva_col1.begin()]) {
            dis_max_value_font = distance_eva_col0[maxPos0 - distance_eva_col0.begin()];
            printf("\n      Result: distance_eva_col0: the %dth number and the max value is %f\n", maxPos0 - distance_eva_col0.begin(),
                   distance_eva_col0[maxPos0 - distance_eva_col0.begin()]);
        }
        else {
            dis_max_value_font = distance_eva_col1[maxPos1 - distance_eva_col1.begin()];
            printf("\n      Result: distance_eva_col1: the %dth number and the max value is %f\n", maxPos1 - distance_eva_col1.begin(),
                   distance_eva_col1[maxPos1 - distance_eva_col1.begin()]);
        }
    }

    else if(distance_eva_col0[maxPosition0 - distance_eva_col0.begin()] <= distance_eva_col1[maxPosition1 - distance_eva_col1.begin()]){
        //最大距离是第二列的某一位置
        keyframe[maxPosition1 - distance_eva_col1.begin()] = keyframe[maxPosition1 - distance_eva_col1.begin()] + 1;
        if(maxPosition1 - distance_eva_col1.begin() == keyframe.size() - 1) {
            if(keyframe[maxPosition1 - distance_eva_col1.begin()] == sf_sum -1) {
                distance_eva_col1[maxPosition1 - distance_eva_col1.begin()] = 0;

                calculate_total_evalation->Cal_Evalation_Update(keyframe[maxPosition1 - distance_eva_col1.begin() - 1],
                                                                keyframe[maxPosition1 - distance_eva_col1.begin()], sf_sum);
                distance_eva_col0[maxPosition1 - distance_eva_col1.begin()] = calculate_total_evalation->evalate_dis_one[0];
                distance_eva_col1[maxPosition1 - distance_eva_col1.begin() - 1] = calculate_total_evalation->evalate_dis_one[1];
            }
            else {
                calculate_total_evalation->Cal_Evalation_Update(keyframe[maxPosition1 - distance_eva_col1.begin()] - 1, sf_sum, sf_sum);
                distance_eva_col1[maxPosition1 - distance_eva_col1.begin()] = calculate_total_evalation->evalate_dis_one[1];

                calculate_total_evalation->Cal_Evalation_Update(keyframe[maxPosition1 - distance_eva_col1.begin() - 1],
                                                                keyframe[maxPosition1 - distance_eva_col1.begin()], sf_sum);
                distance_eva_col0[maxPosition1 - distance_eva_col1.begin()] = calculate_total_evalation->evalate_dis_one[0];
                distance_eva_col1[maxPosition1 - distance_eva_col1.begin() - 1] = calculate_total_evalation->evalate_dis_one[1];
            }
        }
        else if ( maxPosition1 - distance_eva_col1.begin() == 0) {
            if(keyframe[maxPosition1 - distance_eva_col1.begin() + 1] - keyframe[maxPosition1 - distance_eva_col1.begin()] == 1) {
                distance_eva_col0[maxPosition1 - distance_eva_col1.begin() + 1] = 0;
                distance_eva_col1[maxPosition1 - distance_eva_col1.begin()] = 0;

                calculate_total_evalation->Cal_Evalation_Update(0, keyframe[maxPosition1 - distance_eva_col1.begin()] + 1, sf_sum);
                distance_eva_col0[maxPosition1 - distance_eva_col1.begin()] = calculate_total_evalation->evalate_dis_one[0];
            }
            else {
                calculate_total_evalation->Cal_Evalation_Update(0, keyframe[maxPosition1 - distance_eva_col1.begin()]+1, sf_sum);
                distance_eva_col0[maxPosition1 - distance_eva_col1.begin()] = calculate_total_evalation->evalate_dis_one[0];

                calculate_total_evalation->Cal_Evalation_Update(keyframe[maxPosition1 - distance_eva_col1.begin()],
                                                                keyframe[maxPosition1 - distance_eva_col1.begin() + 1], sf_sum);
                distance_eva_col0[maxPosition1 - distance_eva_col1.begin() + 1] = calculate_total_evalation->evalate_dis_one[0];
                distance_eva_col1[maxPosition1 - distance_eva_col1.begin()] = calculate_total_evalation->evalate_dis_one[1];
            }
        }
        else {
            if(keyframe[maxPosition1 - distance_eva_col1.begin() + 1] - keyframe[maxPosition1 - distance_eva_col1.begin()] == 1) {
                distance_eva_col0[maxPosition1 - distance_eva_col1.begin() + 1] = 0;
                distance_eva_col1[maxPosition1 - distance_eva_col1.begin()] = 0;

                calculate_total_evalation->Cal_Evalation_Update(keyframe[maxPosition1 - distance_eva_col1.begin() - 1],
                                                                keyframe[maxPosition1 - distance_eva_col1.begin()], sf_sum);
                distance_eva_col0[maxPosition1 - distance_eva_col1.begin()] = calculate_total_evalation->evalate_dis_one[0];
                distance_eva_col1[maxPosition1 - distance_eva_col1.begin() - 1] = calculate_total_evalation->evalate_dis_one[1];
            }
            else {
                calculate_total_evalation->Cal_Evalation_Update(keyframe[maxPosition1 - distance_eva_col1.begin() - 1],
                                                                keyframe[maxPosition1 - distance_eva_col1.begin()], sf_sum);
                distance_eva_col0[maxPosition1 - distance_eva_col1.begin()] = calculate_total_evalation->evalate_dis_one[0];
                distance_eva_col1[maxPosition1 - distance_eva_col1.begin() - 1] = calculate_total_evalation->evalate_dis_one[1];

                calculate_total_evalation->Cal_Evalation_Update(keyframe[maxPosition1 - distance_eva_col1.begin()],
                                                                keyframe[maxPosition1 - distance_eva_col1.begin() + 1], sf_sum);
                distance_eva_col0[maxPosition1 - distance_eva_col1.begin() + 1] = calculate_total_evalation->evalate_dis_one[0];
                distance_eva_col1[maxPosition1 - distance_eva_col1.begin()] = calculate_total_evalation->evalate_dis_one[1];
            }
        }
        auto maxPos0 = max_element(distance_eva_col0.begin(), distance_eva_col0.end());
        auto maxPos1 = max_element(distance_eva_col1.begin(), distance_eva_col1.end());
        if(distance_eva_col0[maxPos0 - distance_eva_col0.begin()] > distance_eva_col1[maxPos1 - distance_eva_col1.begin()]) {
            dis_max_value_font = distance_eva_col0[maxPos0 - distance_eva_col0.begin()];
            printf("\n     Rusult: distance_eva_col0: the %dth number and the max value is %f\n", maxPos0 - distance_eva_col0.begin(),
                   distance_eva_col0[maxPos0 - distance_eva_col0.begin()]);
        }
        else {
            dis_max_value_font = distance_eva_col1[maxPos1 - distance_eva_col1.begin()];
            printf("\n    Rusult: distance_eva_col1: the %dth number and the max value is %f\n", maxPos1 - distance_eva_col1.begin(),
                   distance_eva_col1[maxPos1 - distance_eva_col1.begin()]);
        }

    }

//    output_vector.Vec1dint_Output(keyframe);
//    output_vector.Vec1ddouble_Output(distance_eva_col0);
//    output_vector.Vec1ddouble_Output(distance_eva_col1);

    delete calculate_total_evalation;
}



//Moving_TPoint_Update函数的作用是优化版：利用的cal的数据为Cal_Evalate_Update_TP函数;
// 交替更新关键帧和距离矩阵（每迭代一次该函数仅仅需要计算四个值，其他的不需要运算）
void Moving_Point::Moving_TPoint_Update(int sf_sum)
{
  Calculate_Total_Evalation *calculate_total_evalation = new Calculate_Total_Evalation;
  auto maxPosition0 = max_element(distance_eva_col0.begin(), distance_eva_col0.end());
  auto maxPosition1 = max_element(distance_eva_col1.begin(), distance_eva_col1.end());
  //前进方向更新规则
  if(distance_eva_col0[maxPosition0 - distance_eva_col0.begin()] > distance_eva_col1[maxPosition1 - distance_eva_col1.begin()])
  {
    if(maxPosition0 - distance_eva_col0.begin() == 0) {
      //printf("		output the position of the max1 vaule: %d \n ", maxPosition0 - distance_eva_col0.begin());
      calculate_total_evalation->Cal_Evalation_Update_TP(keyframe, maxPosition0 - distance_eva_col0.begin(), -1, sf_sum);
      keyframe[maxPosition0 - distance_eva_col0.begin()] = keyframe[maxPosition0 - distance_eva_col0.begin()] - 1;
      distance_eva_col0[maxPosition0 - distance_eva_col0.begin() ] = calculate_total_evalation->evalate_dis_four[0][0];
      distance_eva_col0[maxPosition0 - distance_eva_col0.begin() + 1] = calculate_total_evalation->evalate_dis_four[1][0];
      distance_eva_col1[maxPosition0 - distance_eva_col0.begin() ] = calculate_total_evalation->evalate_dis_four[1][1];
    }
    else {
      //printf("		output the position of the max1 vaule: %d \n ", maxPosition0 - distance_eva_col0.begin());
      //提供两个向量存储一些中间过程的数据
      std::vector<double> dist1(4); double dist_temp1,dist_temp2;
      std::vector<double> dist2(4);

      calculate_total_evalation->Cal_Evalation_Update_TP(keyframe, maxPosition0 - distance_eva_col0.begin() - 1, 1, sf_sum);
      dist1[0] = calculate_total_evalation->evalate_dis_four[0][0];
      dist1[1] = calculate_total_evalation->evalate_dis_four[0][1];
      dist1[2] = calculate_total_evalation->evalate_dis_four[1][0];
      dist1[3] = calculate_total_evalation->evalate_dis_four[1][1];

      //printf("		ouput dist1 data: 1st %f, 2st %f, 3st %f, 4st %f \n ", dist1[0], dist1[1], dist1[2], dist1[3]);
      if(dist1[0] < dist1[1]) {
        dist_temp1 = dist1[1];
      }else{
        dist_temp1 = dist1[0];
      }

      calculate_total_evalation->Cal_Evalation_Update_TP(keyframe, maxPosition0 - distance_eva_col0.begin(), -1, sf_sum);
      dist2[0] = calculate_total_evalation->evalate_dis_four[0][0];
      dist2[1] = calculate_total_evalation->evalate_dis_four[0][1];
      dist2[2] = calculate_total_evalation->evalate_dis_four[1][0];
      dist2[3] = calculate_total_evalation->evalate_dis_four[1][1];
      //printf("		ouput dist2 data: 1st %f, 2st %f, 3st %f, 4st %f \n ", dist2[0], dist2[1], dist2[2], dist2[3]);
      if(dist2[2] < dist2[3]) {
        dist_temp2 = dist2[3];
      }else{
        dist_temp2 = dist2[2];
      }

      if(dist_temp1 > dist_temp2) {//目的是判断选择上一关键帧加一，还是下一关键帧减一操作
        keyframe[maxPosition0 - distance_eva_col0.begin()] = keyframe[maxPosition0 - distance_eva_col0.begin()] - 1;
        distance_eva_col0[maxPosition0 - distance_eva_col0.begin()] = dist2[0];
        distance_eva_col1[maxPosition0 - distance_eva_col0.begin() - 1] = dist2[1];
        distance_eva_col1[maxPosition0 - distance_eva_col0.begin()] = dist2[3];
        if(maxPosition0 - distance_eva_col0.begin() < keyframe.size()-1)
          distance_eva_col0[maxPosition0 - distance_eva_col0.begin() + 1] = dist2[2];
      }
      else{
        keyframe[maxPosition0 - distance_eva_col0.begin() - 1] = keyframe[maxPosition0 - distance_eva_col0.begin() - 1] + 1;
        distance_eva_col0[maxPosition0 - distance_eva_col0.begin() - 1] = dist1[0];
        distance_eva_col0[maxPosition0 - distance_eva_col0.begin()] = dist1[2];
        distance_eva_col1[maxPosition0 - distance_eva_col0.begin() - 2] = dist1[1];
        distance_eva_col1[maxPosition0 - distance_eva_col0.begin() - 1] = dist1[3];
      }
    }
  }
  // 后退方向更新规则
  else{
    if( maxPosition1 - distance_eva_col1.begin() == keyframe.size()-1) {
      //printf("		output the position of the max2 vaule: %d \n ", maxPosition1 - distance_eva_col1.begin());
      calculate_total_evalation->Cal_Evalation_Update_TP(keyframe, maxPosition1 - distance_eva_col1.begin(), 1, sf_sum);
      keyframe[maxPosition1 - distance_eva_col1.begin()] = keyframe[maxPosition1 - distance_eva_col1.begin()] + 1;
      distance_eva_col1[maxPosition1 - distance_eva_col1.begin() ] = calculate_total_evalation->evalate_dis_four[1][1];
      distance_eva_col1[maxPosition1 - distance_eva_col1.begin() - 1] = calculate_total_evalation->evalate_dis_four[0][1];
      distance_eva_col0[maxPosition1 - distance_eva_col1.begin()] = calculate_total_evalation->evalate_dis_four[0][0];
    }
    else {
      //printf("		output the position of the max2 vaule: %d \n ", maxPosition1 - distance_eva_col1.begin());
      //提供两个向量存储一些中间过程的数据
      std::vector<double> dist1(4); double dist_temp1,dist_temp2;
      std::vector<double> dist2(4);

      calculate_total_evalation->Cal_Evalation_Update_TP(keyframe, maxPosition1 - distance_eva_col1.begin(), 1, sf_sum);
      dist1[0] = calculate_total_evalation->evalate_dis_four[0][0];
      dist1[1] = calculate_total_evalation->evalate_dis_four[0][1];
      dist1[2] = calculate_total_evalation->evalate_dis_four[1][0];
      dist1[3] = calculate_total_evalation->evalate_dis_four[1][1];

      //printf("		ouput2 dist1 data: 1st %f, 2st %f, 3st %f, 4st %f \n ", dist1[0], dist1[1], dist1[2], dist1[3]);

      if(dist1[0] < dist1[1]) {
        dist_temp1 = dist1[1];
      }else{
        dist_temp1 = dist1[0];
      }

      calculate_total_evalation->Cal_Evalation_Update_TP(keyframe, maxPosition1 - distance_eva_col1.begin() + 1, -1, sf_sum);
      dist2[0] = calculate_total_evalation->evalate_dis_four[0][0];
      dist2[1] = calculate_total_evalation->evalate_dis_four[0][1];
      dist2[2] = calculate_total_evalation->evalate_dis_four[1][0];
      dist2[3] = calculate_total_evalation->evalate_dis_four[1][1];
      //printf("		ouput2 dist2 data: 1st %f, 2st %f, 3st %f, 4st %f \n ", dist2[0], dist2[1], dist2[2], dist2[3]);
      if(dist2[2] < dist2[3]) {
        dist_temp2 = dist2[3];
      }else{
        dist_temp2 = dist2[2];
      }
      if(dist_temp1 > dist_temp2) {//目的是判断选择上一帧加一，还是下一帧减一操作
        keyframe[maxPosition1 - distance_eva_col1.begin() + 1] = keyframe[maxPosition1 - distance_eva_col1.begin() + 1] - 1;
        distance_eva_col0[maxPosition1 - distance_eva_col1.begin() + 1] = dist2[0];
        distance_eva_col0[maxPosition1 - distance_eva_col1.begin() + 2] = dist2[2];
        distance_eva_col1[maxPosition1 - distance_eva_col1.begin()] = dist2[1];
        distance_eva_col1[maxPosition1 - distance_eva_col1.begin() + 1] = dist2[3];
      }
      else{
        keyframe[maxPosition1 - distance_eva_col1.begin()] = keyframe[maxPosition1 - distance_eva_col1.begin()] + 1;
        distance_eva_col0[maxPosition1 - distance_eva_col1.begin() ]  = dist1[0];
        distance_eva_col0[maxPosition1 - distance_eva_col1.begin() + 1] = dist1[2];
        distance_eva_col1[maxPosition1 - distance_eva_col1.begin() ] = dist1[3];
        if(maxPosition1 - distance_eva_col1.begin() > 0)
          distance_eva_col1[maxPosition1 - distance_eva_col1.begin() - 1] = dist1[1];
      }
    }
  }

  auto maxPos0 = max_element(distance_eva_col0.begin(), distance_eva_col0.end());
  auto maxPos1 = max_element(distance_eva_col1.begin(), distance_eva_col1.end());
  if(distance_eva_col0[maxPos0 - distance_eva_col0.begin()] > distance_eva_col1[maxPos1 - distance_eva_col1.begin()]) {
    dis_max_value_font = distance_eva_col0[maxPos0 - distance_eva_col0.begin()];
    printf("		Rusult: distance_eva_col0: the %dth number and the max value is %f\n",
           maxPos0 - distance_eva_col0.begin(), distance_eva_col0[maxPos0 - distance_eva_col0.begin()]);
  }
  else {
    dis_max_value_font = distance_eva_col1[maxPos1 - distance_eva_col1.begin()];
    printf("		Rusult: distance_eva_col1: the %dth number and the max value is %f\n",
           maxPos1 - distance_eva_col1.begin(), distance_eva_col1[maxPos1 - distance_eva_col1.begin()]);
  }

//  output_vector.Vec1dint_Output(keyframe);
//  output_vector.Vec1ddouble_Output(distance_eva_col0);
//  output_vector.Vec1ddouble_Output(distance_eva_col1);

}	


//Moving_NTPoint_Update函数的作用是最终优化版：加入同时移动多个关键帧的功能,我们需要带有反馈功能并使得距离严格下降的算法
void Moving_Point::Moving_NTPoint_Update(int sf_sum)
{
	Calculate_Total_Evalation *calculate_total_evalation = new Calculate_Total_Evalation;
	auto maxPosition0 = max_element(distance_eva_col0.begin(), distance_eva_col0.end());
	auto maxPosition1 = max_element(distance_eva_col1.begin(), distance_eva_col1.end());
	std::vector<double> dist1;
	std::vector<double> dist2;
	double dis_temp,dist_tem1,dist_temp2;
	int remove_N; //同时移动关键帧的数量
    //关键帧左移
	if(distance_eva_col0[maxPosition0 - distance_eva_col0.begin()] > distance_eva_col1[maxPosition1 - distance_eva_col1.begin()]) 
	{
		printf("Access One: +++ \n");
		//第一关键帧
		if(maxPosition0 - distance_eva_col0.begin() == 0) {
			remove_N = 1;  // 
			dist1.resize(remove_N+1);
			dist2.resize(remove_N+1);

			calculate_total_evalation->Cal_Evalation_Update_TPN(keyframe, maxPosition0 - distance_eva_col0.begin(), -1, sf_sum, remove_N);
			dist1[0] = calculate_total_evalation->evalate_dis_removeN[0][0]; // calculate_total_evalation->evalate_dis_removeN[][2]
			dist1[1] = calculate_total_evalation->evalate_dis_removeN[1][0];
			dist2[0] = calculate_total_evalation->evalate_dis_removeN[0][1];
			dist2[1] = calculate_total_evalation->evalate_dis_removeN[1][1];
			dis_temp = max(max(dist1[0],dist1[1]),max(dist2[0],dist2[1]));
			if( dis_temp < distance_eva_col0[maxPosition0 - distance_eva_col0.begin()]) {
				keyframe[maxPosition0 - distance_eva_col0.begin()] = keyframe[maxPosition0 - distance_eva_col0.begin()] - 1;
				distance_eva_col0[maxPosition0 - distance_eva_col0.begin()] = dist1[1];
				distance_eva_col0[maxPosition0 - distance_eva_col0.begin() + 1] = dist1[0];
				distance_eva_col1[maxPosition0 - distance_eva_col0.begin()] = dist2[0];
			}
			else{
				for(int Rvi=2; Rvi<keyframe.size()-1; Rvi++){	
					remove_N = Rvi;
					dist1.resize(remove_N+1);
					dist2.resize(remove_N+1);
					calculate_total_evalation->Cal_Evalation_Update_TPN(keyframe, maxPosition0 - distance_eva_col0.begin(), -1, sf_sum, remove_N);
					for(int i=0; i<remove_N + 1; i++){
						dist1[i] = calculate_total_evalation->evalate_dis_removeN[i][0];
						dist2[i] = calculate_total_evalation->evalate_dis_removeN[i][1];
					}
					auto maxPos0 = max_element(dist1.begin(), dist1.end());
					auto maxPos1 = max_element(dist2.begin(), dist2.end());
					if(*maxPos0 > *maxPos1) dis_temp = *maxPos0; else dis_temp = *maxPos1;
					if(dis_temp < distance_eva_col0[maxPosition0 - distance_eva_col0.begin()]) {
						for(int k = 0; k < remove_N; k++){
							keyframe[maxPosition0 - distance_eva_col0.begin() + k] = keyframe[maxPosition0 - distance_eva_col0.begin() + k] - 1;
							distance_eva_col0[maxPosition0 - distance_eva_col0.begin() + k] = calculate_total_evalation->evalate_dis_removeN[k][0];
							distance_eva_col1[maxPosition0 - distance_eva_col0.begin() + k] = calculate_total_evalation->evalate_dis_removeN[k][1];
						}
						distance_eva_col0[maxPosition0 - distance_eva_col0.begin() + remove_N] = calculate_total_evalation->evalate_dis_removeN[remove_N][0];
						distance_eva_col1[maxPosition0 - distance_eva_col0.begin() + remove_N] = calculate_total_evalation->evalate_dis_removeN[remove_N][1];
						break;
					}
					else if(Rvi == keyframe.size() - 2){
						printf("Sorry, we have get a best result...\n");
					}
				}
			}
		}
		//非第一的其他关键帧
		else {
			remove_N = 1;  // 
			dist1.resize(remove_N+1);
			dist2.resize(remove_N+1);

			calculate_total_evalation->Cal_Evalation_Update_TPN(keyframe, maxPosition0 - distance_eva_col0.begin(), -1, sf_sum, remove_N);
			dist1[0] = calculate_total_evalation->evalate_dis_removeN[0][0];
			dist1[1] = calculate_total_evalation->evalate_dis_removeN[0][1];
			dist2[0] = calculate_total_evalation->evalate_dis_removeN[1][0];
			dist2[1] = calculate_total_evalation->evalate_dis_removeN[1][1];
			dis_temp = max(max(dist1[0],dist1[1]),max(dist2[0],dist2[1]));
			//一步初始判断
			if( dis_temp < distance_eva_col0[maxPosition0 - distance_eva_col0.begin()]) {
				keyframe[maxPosition0 - distance_eva_col0.begin()] = keyframe[maxPosition0 - distance_eva_col0.begin()] - 1;
				distance_eva_col0[maxPosition0 - distance_eva_col0.begin()] = dist1[1];
				distance_eva_col0[maxPosition0 - distance_eva_col0.begin() + 1] = dist1[0];
				distance_eva_col1[maxPosition0 - distance_eva_col0.begin()] = dist2[0];
				distance_eva_col1[maxPosition0 - distance_eva_col0.begin() - 1] = dist2[1];
			}
			else{
				int label = 1;
				for(int Rvi=2; Rvi < keyframe.size() - 1 -(maxPosition0 - distance_eva_col0.begin()); Rvi++){	
					remove_N = Rvi;
					dist1.resize(remove_N+1);
					dist2.resize(remove_N+1);
					calculate_total_evalation->Cal_Evalation_Update_TPN(keyframe, maxPosition0 - distance_eva_col0.begin(), -1, sf_sum, remove_N);
					for(int i=0; i<remove_N + 1; i++){
						dist1[i] = calculate_total_evalation->evalate_dis_removeN[i][0];
						dist2[i] = calculate_total_evalation->evalate_dis_removeN[i][1];
					}
					auto maxPos0 = max_element(dist1.begin(), dist1.end());
					auto maxPos1 = max_element(dist2.begin(), dist2.end());
					if(*maxPos0 > *maxPos1) dis_temp = *maxPos0; else dis_temp = *maxPos1;
					if(dis_temp < distance_eva_col0[maxPosition0 - distance_eva_col0.begin()]) {
						for(int k = 0; k < remove_N; k++){
							keyframe[maxPosition0 - distance_eva_col0.begin() + k] = keyframe[maxPosition0 - distance_eva_col0.begin() + k] - 1;
							distance_eva_col0[maxPosition0 - distance_eva_col0.begin() + k] = calculate_total_evalation->evalate_dis_removeN[k][0];
							distance_eva_col1[maxPosition0 - distance_eva_col0.begin() + k] = calculate_total_evalation->evalate_dis_removeN[k][1];
						}
						distance_eva_col0[maxPosition0 - distance_eva_col0.begin() + remove_N] = calculate_total_evalation->evalate_dis_removeN[remove_N][0];
						distance_eva_col1[maxPosition0 - distance_eva_col0.begin() + remove_N] = calculate_total_evalation->evalate_dis_removeN[remove_N][1];
						label = -1; // 成功找到，贴个标签
						break;
					}
					else if(Rvi == keyframe.size() - 2){
						label = 0;
					}
				}
				if(label == 0 || label == 1){
					for(int Rvi = 2; Rvi < maxPosition0 - distance_eva_col0.begin() + 1; Rvi++){	
						remove_N = Rvi;
						dist1.resize(remove_N+1);
						dist2.resize(remove_N+1);
						calculate_total_evalation->Cal_Evalation_Update_TPN(keyframe, maxPosition0 - distance_eva_col0.begin(), 1, sf_sum, remove_N);
						for(int i=0; i<remove_N + 1; i++){
							dist1[i] = calculate_total_evalation->evalate_dis_removeN[i][0];
							dist2[i] = calculate_total_evalation->evalate_dis_removeN[i][1];
						}
						auto maxPos0 = max_element(dist1.begin(), dist1.end());
						auto maxPos1 = max_element(dist2.begin(), dist2.end());
						if(*maxPos0 > *maxPos1) dis_temp = *maxPos0; else dis_temp = *maxPos1;
						if(dis_temp < distance_eva_col0[maxPosition0 - distance_eva_col0.begin()]) {
							for(int k = 0; k < remove_N; k++){
								keyframe[maxPosition0 - distance_eva_col0.begin() - k] = keyframe[maxPosition0 - distance_eva_col0.begin() - k] + 1;
								distance_eva_col0[maxPosition0 - distance_eva_col0.begin() - k] = calculate_total_evalation->evalate_dis_removeN[k][0];
								distance_eva_col1[maxPosition0 - distance_eva_col0.begin() - k] = calculate_total_evalation->evalate_dis_removeN[k][1];
							}
							distance_eva_col0[maxPosition0 - distance_eva_col0.begin() - remove_N] = calculate_total_evalation->evalate_dis_removeN[remove_N][0];
							distance_eva_col1[maxPosition0 - distance_eva_col0.begin() - remove_N] = calculate_total_evalation->evalate_dis_removeN[remove_N][1];
							label = -1; // 成功找到，贴个标签
							break;
						}
						else if(Rvi == keyframe.size() - 2){
							printf("Sorry, we have no way \n");
						}
					}
				}
			}
		}
	}
	//关键帧右移
	else{ //此时是选择了第二列的情形
		printf("Access Two: \n");
		if( maxPosition1 - distance_eva_col1.begin() == keyframe.size()-1) {
			printf("	last frame:\n");
			remove_N = 1;  // 
			dist1.resize(remove_N+1);
			dist2.resize(remove_N+1);

			calculate_total_evalation->Cal_Evalation_Update_TPN_second(keyframe, maxPosition1 - distance_eva_col1.begin(), 1, sf_sum, remove_N);
			dist1[0] = calculate_total_evalation->evalate_dis_removeN[0][0];
			dist1[1] = calculate_total_evalation->evalate_dis_removeN[0][1];
			dist2[0] = calculate_total_evalation->evalate_dis_removeN[1][0];
			dist2[1] = calculate_total_evalation->evalate_dis_removeN[1][1];
			dis_temp = max(max(dist1[0],dist1[1]),max(dist2[0],dist2[1]));
			if( dis_temp < distance_eva_col1[maxPosition1 - distance_eva_col1.begin()]) {
				keyframe[maxPosition1 - distance_eva_col1.begin()] = keyframe[maxPosition1 - distance_eva_col1.begin()] - 1;
				distance_eva_col0[maxPosition1 - distance_eva_col1.begin()] = dist1[0];
				distance_eva_col1[maxPosition1 - distance_eva_col1.begin() - 1] = dist2[1];
				distance_eva_col1[maxPosition1 - distance_eva_col1.begin()] = dist2[0];
				printf("	Output the results: dist1,dist2,%f,%f,%f,%f \n", dist1[0],dist1[1], dist2[0], dist2[1]);
			}
			else{
				for(int Rvi=2; Rvi<keyframe.size()-1; Rvi++){	
					remove_N = Rvi;
					dist1.resize(remove_N+1);
					dist2.resize(remove_N+1);
					calculate_total_evalation->Cal_Evalation_Update_TPN_second(keyframe, maxPosition1 - distance_eva_col1.begin(), 1, sf_sum, remove_N);
					for(int i=0; i<remove_N + 1; i++){
						dist1[i] = calculate_total_evalation->evalate_dis_removeN[i][0];
						dist2[i] = calculate_total_evalation->evalate_dis_removeN[i][1];
					}
					auto maxPos0 = max_element(dist1.begin(), dist1.end());
					auto maxPos1 = max_element(dist2.begin(), dist2.end());
					if(*maxPos0 > *maxPos1) dis_temp = *maxPos0; else dis_temp = *maxPos1;
					if(dis_temp < distance_eva_col0[maxPosition1 - distance_eva_col1.begin()]) {
						keyframe[maxPosition1 - distance_eva_col1.begin()] = keyframe[maxPosition1 - distance_eva_col1.begin()] - 1;
						if(maxPosition1 - distance_eva_col1.begin() < keyframe.size() -1){
							distance_eva_col0[maxPosition1 - distance_eva_col1.begin() + 1] = calculate_total_evalation->evalate_dis_removeN[0][0];
						}
						distance_eva_col1[maxPosition1 - distance_eva_col1.begin()] = calculate_total_evalation->evalate_dis_removeN[0][1];
						for(int k = 1; k < remove_N; k++){
							keyframe[maxPosition1 - distance_eva_col1.begin() - k] = keyframe[maxPosition1 - distance_eva_col1.begin() - k] - 1;
							distance_eva_col0[maxPosition1 - distance_eva_col1.begin() - k + 1] = calculate_total_evalation->evalate_dis_removeN[k][0];
							distance_eva_col1[maxPosition1 - distance_eva_col1.begin() - k] = calculate_total_evalation->evalate_dis_removeN[k][1];
						}	
						keyframe[maxPosition0 - distance_eva_col1.begin() - remove_N] = keyframe[maxPosition1 - distance_eva_col1.begin() - remove_N] - 1;
						distance_eva_col0[maxPosition1 - distance_eva_col1.begin() - remove_N + 1] = calculate_total_evalation->evalate_dis_removeN[remove_N][0];
						if(maxPosition1 - distance_eva_col1.begin() - remove_N >= 0) {
							distance_eva_col1[maxPosition1 - distance_eva_col1.begin() - remove_N] = calculate_total_evalation->evalate_dis_removeN[remove_N][1];
						}
						printf("	We choose the remove_N is %d\n", remove_N);
						break;
					}
					else if(Rvi == keyframe.size() - 2){
						printf("Sorry, we have get a best result...\n");
					}
				}
			}
		}
		else {
			printf("	Not last frame:\n");
			remove_N = 1;  // 
			dist1.resize(remove_N+1);
			dist2.resize(remove_N+1);
				
			printf("	Output the second column data: %d\n", maxPosition1 - distance_eva_col1.begin());
			remove_N = 1;  // 
			calculate_total_evalation->Cal_Evalation_Update_TPN_second(keyframe, maxPosition1 - distance_eva_col1.begin(), -1, sf_sum, remove_N);
			dist1[0] = calculate_total_evalation->evalate_dis_removeN[0][0];
			dist1[1] = calculate_total_evalation->evalate_dis_removeN[1][0];
			dist2[0] = calculate_total_evalation->evalate_dis_removeN[0][1];
			dist2[1] = calculate_total_evalation->evalate_dis_removeN[1][1];
			dis_temp = max(max(dist1[0],dist1[1]),max(dist2[0],dist2[1]));
			printf("	Output the max value when removing: %f\n", dis_temp);
			if( dis_temp < distance_eva_col1[maxPosition1 - distance_eva_col1.begin()]) {
				keyframe[maxPosition1 - distance_eva_col1.begin() + 1] = keyframe[maxPosition1 - distance_eva_col1.begin() + 1] - 1;
				distance_eva_col0[maxPosition1 - distance_eva_col1.begin() + 1] = dist1[0];
				if(maxPosition1 - distance_eva_col1.begin() < keyframe.size() - 2){
					distance_eva_col0[maxPosition1 - distance_eva_col1.begin() + 2] = dist1[1];
				}
				distance_eva_col1[maxPosition1 - distance_eva_col1.begin() + 1] = dist2[1];
				distance_eva_col1[maxPosition1 - distance_eva_col1.begin() ] = dist2[0];
				printf("	Output the distance: %f, %f , %f, %f \n", dist1[0],dist2[0], dist1[1], dist2[1]);
			}
			else{
				printf(" Output other case: \n");
				int label = 1;
				for(int Rvi=2; Rvi<(maxPosition1 - distance_eva_col1.begin()) + 1; Rvi++){	
					remove_N = Rvi;
					dist1.resize(remove_N+1);
					dist2.resize(remove_N+1);
					calculate_total_evalation->Cal_Evalation_Update_TPN_second(keyframe, maxPosition1 - distance_eva_col1.begin(), -1, sf_sum, remove_N);
					for(int i=0; i<remove_N + 1; i++){
						dist1[i] = calculate_total_evalation->evalate_dis_removeN[i][0];
						dist2[i] = calculate_total_evalation->evalate_dis_removeN[i][1];
					}
					auto maxPos0 = max_element(dist1.begin(), dist1.end());
					auto maxPos1 = max_element(dist2.begin(), dist2.end());
					if(*maxPos0 > *maxPos1) dis_temp = *maxPos0; else dis_temp = *maxPos1;
					if(dis_temp < distance_eva_col1[maxPosition1 - distance_eva_col1.begin()]) {
						for(int k = 0; k < remove_N; k++){
							keyframe[maxPosition1 - distance_eva_col1.begin() + k] = keyframe[maxPosition1 - distance_eva_col1.begin() + k] - 1;
							distance_eva_col0[maxPosition1 - distance_eva_col1.begin() + k] = calculate_total_evalation->evalate_dis_removeN[k][0];
							distance_eva_col1[maxPosition1 - distance_eva_col1.begin() + k] = calculate_total_evalation->evalate_dis_removeN[k][1];
						}
						distance_eva_col0[maxPosition1 - distance_eva_col1.begin() + remove_N] = calculate_total_evalation->evalate_dis_removeN[remove_N][0];
						distance_eva_col1[maxPosition1 - distance_eva_col1.begin() + remove_N] = calculate_total_evalation->evalate_dis_removeN[remove_N][1];
						label = -1; // 成功找到，贴个标签
						break;
					}
					else if(Rvi == maxPosition1 - distance_eva_col1.begin()){
						label = 0;
					}
				}
				if(label == 0 || label == 1){
					for(int Rvi = 2; Rvi < maxPosition1 - distance_eva_col1.begin() + 1; Rvi++){	
						remove_N = Rvi;
						dist1.resize(remove_N+1);
						dist2.resize(remove_N+1);
						calculate_total_evalation->Cal_Evalation_Update_TPN_second(keyframe, maxPosition1 - distance_eva_col1.begin(), 1, sf_sum, remove_N);
						for(int i=0; i<remove_N + 1; i++){
							dist1[i] = calculate_total_evalation->evalate_dis_removeN[i][0];
							dist2[i] = calculate_total_evalation->evalate_dis_removeN[i][1];
						}
						auto maxPos0 = max_element(dist1.begin(), dist1.end());
						auto maxPos1 = max_element(dist2.begin(), dist2.end());
						if(*maxPos0 > *maxPos1) dis_temp = *maxPos0; else dis_temp = *maxPos1;
						if(dis_temp < distance_eva_col1[maxPosition1 - distance_eva_col1.begin()]) {
							for(int k = 0; k < remove_N; k++){
								keyframe[maxPosition1 - distance_eva_col1.begin() - k] = keyframe[maxPosition1 - distance_eva_col1.begin() - k] + 1;
								distance_eva_col0[maxPosition1 - distance_eva_col1.begin() - k] = calculate_total_evalation->evalate_dis_removeN[k][0];
								distance_eva_col1[maxPosition1 - distance_eva_col1.begin() - k] = calculate_total_evalation->evalate_dis_removeN[k][1];
							}
							distance_eva_col0[maxPosition1 - distance_eva_col1.begin() - remove_N] = calculate_total_evalation->evalate_dis_removeN[remove_N][0];
							distance_eva_col1[maxPosition1 - distance_eva_col1.begin() - remove_N] = calculate_total_evalation->evalate_dis_removeN[remove_N][1];
							label = -1; // 成功找到，贴个标签
							break;
						}
						else if(Rvi == keyframe.size() - 2){
							printf("Sorry, we have no way \n");
						}
					}
				}
			}
		}
	}
	auto maxPos0 = max_element(distance_eva_col0.begin(), distance_eva_col0.end());
	auto maxPos1 = max_element(distance_eva_col1.begin(), distance_eva_col1.end());
	if(distance_eva_col0[maxPos0 - distance_eva_col0.begin()] > distance_eva_col1[maxPos1 - distance_eva_col1.begin()]) {
		dis_max_value_font = distance_eva_col0[maxPos0 - distance_eva_col0.begin()];
		printf("		Rusult: distance_eva_col0: the %dth number and the max value is %f\n", maxPos0 - distance_eva_col0.begin(), distance_eva_col0[maxPos0 - distance_eva_col0.begin()]);
	}
	else {
		dis_max_value_font = distance_eva_col1[maxPos1 - distance_eva_col1.begin()];
		printf("		Rusult: distance_eva_col1: the %dth number and the max value is %f\n", maxPos1 - distance_eva_col1.begin(), distance_eva_col1[maxPos1 - distance_eva_col1.begin()]);
	}

//	output_vector.Vec1dint_Output(keyframe);
//	output_vector.Vec1ddouble_Output(distance_eva_col0);
//	output_vector.Vec1ddouble_Output(distance_eva_col1);
}





//Interp_point函数的作用是利用插值关键帧的方法降低目标值：当插入一帧关键帧的时候就要去掉一帧关键帧，最终使得目标值降低
void Moving_Point::Interp_point(int sf_sum)
{
	Calculate_Total_Evalation *calculate_total_evalation = new Calculate_Total_Evalation;
  auto maxPosition0 = max_element(distance_eva_col0.begin(), distance_eva_col0.end());
  auto maxPosition1 = max_element(distance_eva_col1.begin(), distance_eva_col1.end());

  if(distance_eva_col0[maxPosition0 - distance_eva_col0.begin()] > distance_eva_col1[maxPosition1 - distance_eva_col1.begin()]) 
	{
		printf("test1\n");
		calculate_total_evalation->Cal_Evalation_Slack_P(keyframe,sf_sum);  //已经产生了calculate_total_evalation->slack_position数据编号
		int min_fra = 1000.0;// 中间变量，给定足够大的数字，使得总会受到约束
		int min_keyfra_N;
		for(int i=0; i<keyframe.size(); i++){
			calculate_total_evalation->Cal_Evalation_Interp_P(keyframe, i , 1, sf_sum);
			if(min_fra > distance_eva_col0[calculate_total_evalation->interp_position])  {
				min_fra = calculate_total_evalation->interp_position;  // 表示到底插入的是第几帧
				min_keyfra_N = i;                                      // 表示第几关键帧
			}
		}
		//需要调整合适的关键帧，并更新distance_eva_col0和distance_eva_col1的距离数据
		if(min_fra == calculate_total_evalation->slack_position){
			printf("It's hard to optimize the problem, and please adjust your case......");
		}
		else if(keyframe[min_keyfra_N] == calculate_total_evalation->slack_position || keyframe[min_keyfra_N] == calculate_total_evalation->slack_position - 1){
			keyframe[min_keyfra_N] = calculate_total_evalation->slack_position;
		}
		else if(keyframe[min_keyfra_N] < calculate_total_evalation-> slack_position -1){
			for(; ;){
				printf("go on now.");
			}	
		}
		else{
			for(; ;){
				printf("go on now.");
			}	
		}
		calculate_total_evalation->Cal_Evalation(keyframe, sf_sum);//这里直接求解，可以进一步优化！！！！！@lft
	}
	else{
		printf("test2, we start the pra\n");
		calculate_total_evalation->Cal_Evalation_Slack_P(keyframe,sf_sum);  //已经产生了calculate_total_evalation->slack_position数据编号
		printf("test2_1, we get the data about the position of slack point: %d \n", calculate_total_evalation->slack_position);

		int min_fra = 1000;// 中间变量，给定足够大的数字，使得总会受到约束
		int min_keyfra_N;
		for(int i=0; i<keyframe.size(); i++){
			printf("test2_1_1 %d\n", i);
			calculate_total_evalation->Cal_Evalation_Interp_P(keyframe, i , -1, sf_sum);
			printf("test2_1_2 %d\n", i);
			if(min_fra > distance_eva_col0[calculate_total_evalation->interp_position])  {
				min_fra = calculate_total_evalation->interp_position;  // 表示到底插入的是第几帧
				min_keyfra_N = i;                                      // 表示第几关键帧
			}
		}
		//需要调整合适的关键帧，并更新distance_eva_col0和distance_eva_col1的距离数据
		if(min_fra == calculate_total_evalation->slack_position){
			printf("It's hard to optimize the problem, and please adjust your case......");
		}
		else if(keyframe[min_keyfra_N] == calculate_total_evalation->slack_position || keyframe[min_keyfra_N] == calculate_total_evalation->slack_position - 1){
			keyframe[min_keyfra_N] = calculate_total_evalation->slack_position;
		}
		else if(keyframe[min_keyfra_N] < calculate_total_evalation-> slack_position -1){
			for(; ;){
				printf("go on now.");
			}	
		}
		else{
			for(; ;){
				printf("go on now.");
			}
		}
		calculate_total_evalation->Cal_Evalation(keyframe, sf_sum);//这里直接求解，可以进一步优化！！！！！@lft
	}

	auto maxPos0 = max_element(distance_eva_col0.begin(), distance_eva_col0.end());
  auto maxPos1 = max_element(distance_eva_col1.begin(), distance_eva_col1.end());
  if(distance_eva_col0[maxPos0 - distance_eva_col0.begin()] > distance_eva_col1[maxPos1 - distance_eva_col1.begin()]) {
		dis_max_value_font = distance_eva_col0[maxPos0 - distance_eva_col0.begin()];
    printf("		Rusult: distance_eva_col0: the %dth number and the max value is %f\n", maxPos0 - distance_eva_col0.begin(), distance_eva_col0[maxPos0 - distance_eva_col0.begin()]);
  }
  else {
		dis_max_value_font = distance_eva_col1[maxPos1 - distance_eva_col1.begin()];
		printf("		Rusult: distance_eva_col1: the %dth number and the max value is %f\n", maxPos1 - distance_eva_col1.begin(), distance_eva_col1[maxPos1 - distance_eva_col1.begin()]);
  }

//	output_vector.Vec1dint_Output(keyframe);
//	output_vector.Vec1ddouble_Output(distance_eva_col0);
//	output_vector.Vec1ddouble_Output(distance_eva_col1);
}



